function [c,ceq] = mycon(T,S,contype,r)
ceq = [];
c = [0.01 - sqrt((S(1)-S(3))^2 + (S(2)-S(4))^2);
     0.01 - sqrt((S(3)-S(5))^2 + (S(4)-S(6))^2);
     0.01 - sqrt((S(1)-S(5))^2 + (S(2)-S(6))^2);
     ];
 
% contype = 1 观测站布站等边三角形约束
% contype = 2 目标和监测站之间的距离大于 r km
if contype
    ceq = [(S(1)-S(3))^2 + (S(2)-S(4))^2 - (S(3)-S(5))^2 - (S(4)-S(6))^2;
           (S(1)-S(5))^2 + (S(2)-S(6))^2 - (S(3)-S(5))^2 - (S(4)-S(6))^2
          ];
elseif  contype == 2
    c = [c;
         r - sqrt((S(1)-T(1))^2 + (S(2)-T(2))^2);
         r - sqrt((S(3)-T(1))^2 + (S(4)-T(2))^2);
         r - sqrt((S(5)-T(1))^2 + (S(6)-T(2))^2)
         ];
end
end
